/******************************************************************************
 * Copyright 2022 The Untouch Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <QObject>
#include <mutex>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "modules/calibration/lidar_imu_calibrator/include/odometry_frame.h"
#include "modules/localization/msf/common/util/thread_pool.h"

namespace lidar_imu_calibrator {

class OdometryFrameSet : public QObject {
  Q_OBJECT
 public:
  OdometryFrameSet();
  bool StartLoad(const std::string& pcd_folder_path,
                 const std::string& odometry_file_path,
                 const std::string& extrinsic_file_path);
  void StartCompensateData();
  void Stop();
  bool IsNeedCompensate();
  const Eigen::Isometry3d& GetExtrinsicParameter();
  void SelectKeyFrames(float key_frame_delta_x, float key_frame_delta_angle);
  void UpdateDelayTime(double delay_time);
  void UpdateExtrinsics(const Eigen::Isometry3d& extrinsics);
  void DrawPointCloud(glk::GLSLShader& shader,
                      float downsample_resolution = 0.2f);
  bool SaveExtrinsicParameter(const Eigen::Isometry3d& extrinsic,
                              const std::string& file_path);
  void GetFrames(std::vector<OdometryFrame::Ptr> &frames);

 signals:
  void ProgressChanged(int percent);

 private:
  Eigen::Isometry3d InterpolatePose(double timestamp);
  void AddOdometryFrame(OdometryFrame::Ptr& frame);
  void LoadPointCloud(double timestamp, const std::string& pcd_folder_path);
  void CompensatePointCloud(std::shared_ptr<OdometryFrame>& frame);
  void ResetProgress(int total_count);
  bool LoadTimestamps(const std::string& file_path,
                      std::vector<double>& timestamps);
  bool LoadPoses(const std::string& file_path);
  bool LoadExtrinsic(const std::string& file_path);
  std::string TimeToString(double time, int precision = 9);
  void UpdateCompleteStatus();

 private:
  std::mutex progress_mutex_;
  std::mutex frames_mutex_;
  int progress_;
  int total_count_;
  int complete_count_;
  std::unique_ptr<apollo::localization::msf::ThreadPool> thread_pool_;
  std::vector<OdometryFrame::Ptr> frames_;
  std::vector<OdometryFrame::Ptr> key_frames_;
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>
      ins_poses_;
  std::vector<double> ins_timestamps_;
  Eigen::Isometry3d extrinsic_;
  std::string frame_id_;
  std::string child_frame_id_;
};
}  // namespace lidar_imu_calibrator
